/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-07-08     86188       the first version
 */


#include <inc/motor_ctr.h>
#include <inc/pid.h>
#include <inc/main.h>
#include <inc/gpio.h>
#include <math.h>
#include <string.h>





motor_t leftFrontMotor;
motor_t leftBackMotor;
motor_t rightFrontMotor;
motor_t rightBackMotor;


attitude_t desiredPos;
attitude_t actualPos;

attitude_t desiredVel;
attitude_t actualVel;


uint8_t car_sta = STOP;


uint8_t pidx_en = 1;
uint8_t pidy_en = 1;
uint8_t pidz_en = 1;


void carStop(void)
{
    TIM1->CCR1 = 0;
    TIM1->CCR2 = 0;
    TIM1->CCR3 = 0;
    TIM1->CCR4 = 0;
}

void carSetTextPwm(uint16_t p)
{
    if(p > 1000)
        return;

    TIM1->CCR1 = (uint16_t)p;
    TIM1->CCR2 = (uint16_t)p;
    TIM1->CCR3 = (uint16_t)p;
    TIM1->CCR4 = (uint16_t)p;
}


void carSetPwm(void)
{



    TIM1->CCR1 = (uint16_t)leftFrontMotor.outPwm;
    TIM1->CCR2 = (uint16_t)rightFrontMotor.outPwm;
    TIM1->CCR3 = (uint16_t)rightBackMotor.outPwm;
    TIM1->CCR4 = (uint16_t)leftBackMotor.outPwm;


}



void motorPidInit(void)
{
    //vel ctr
    pidReset(&leftFrontPid);
    pidReset(&rightFrontPid);
    pidReset(&rightBackPid);
    pidReset(&leftBackPid);

    pidReset(&xPosPid); //pos ctr
    pidReset(&yPosPid);
    pidReset(&zPosPid);



    pidSetPID(&leftFrontPid,  VEL_KP,  VEL_KI  ,  VEL_KD);
    pidSetPID(&rightFrontPid, VEL_KP,  VEL_KI  ,  VEL_KD);
    pidSetPID(&rightBackPid,  VEL_KP,  VEL_KI  ,  VEL_KD);
    pidSetPID(&leftBackPid,   VEL_KP,  VEL_KI  ,  VEL_KD);

    pidSetPID(&xPosPid,  POSX_KP,  POSX_KI  ,  POSX_KD);
    pidSetPID(&yPosPid,  POSY_KP,  POSY_KI  ,  POSY_KD);
    pidSetPID(&zPosPid,  POSZ_KP,  POSZ_KI  ,  POSZ_KD);



    pidSetIntegralLimit(&leftFrontPid,PID_VEL_INTEGRATION_LIMIT);
    pidSetIntegralLimit(&rightFrontPid,PID_VEL_INTEGRATION_LIMIT);
    pidSetIntegralLimit(&rightBackPid,PID_VEL_INTEGRATION_LIMIT);
    pidSetIntegralLimit(&leftBackPid,PID_VEL_INTEGRATION_LIMIT);

    pidSetIntegralLimit(&xPosPid,PID_POSX_INTEGRATION_LIMIT);
    pidSetIntegralLimit(&yPosPid,PID_POSY_INTEGRATION_LIMIT);
    pidSetIntegralLimit(&zPosPid,PID_POSZ_INTEGRATION_LIMIT);



    pidSetOutLimit(&leftFrontPid,PID_VEL_OUTPUT_LIMIT);
    pidSetOutLimit(&rightFrontPid,PID_VEL_OUTPUT_LIMIT);
    pidSetOutLimit(&rightBackPid,PID_VEL_OUTPUT_LIMIT);
    pidSetOutLimit(&leftBackPid,PID_VEL_OUTPUT_LIMIT);

    pidSetOutLimit(&xPosPid,PID_POSX_OUTPUT_LIMIT);
    pidSetOutLimit(&yPosPid,PID_POSY_OUTPUT_LIMIT);
    pidSetOutLimit(&zPosPid,PID_POSZ_OUTPUT_LIMIT);

}


/*
 *  input : desiredPos.x/y/z    actualPos.x/y/z
 *
 *  out :  outPwm
 *
 * 流程 :
                    1. 输入当前 预期位置值  计算位置误差值
                    2. 进入xyz轴pid输出xyz期望速度值
                    3. 进行数据融合 将xyz期望值加载到四个轮子上
                    4. 以四个轮子期望值 再进行pid 输出pwm值
                    5. 最终放在pwm控制中
*/

void posContorUpdata(void)
{

    if(pidx_en)
        pidUpdate(&xPosPid ,desiredPos.x - actualPos.x);
    else
        xPosPid.out = 0;

    if(pidy_en)
        pidUpdate(&yPosPid ,desiredPos.y - actualPos.y);
    else
        yPosPid.out = 0;

    if(pidz_en)
        pidUpdate(&zPosPid ,desiredPos.z - actualPos.z);
    else
        zPosPid.out = 0;




    leftFrontMotor.outVel   = + xPosPid.out  + yPosPid.out  + zPosPid.out ;
    rightFrontMotor.outVel  = - xPosPid.out  + yPosPid.out  - zPosPid.out ;
    rightBackMotor.outVel   = + xPosPid.out  + yPosPid.out  - zPosPid.out ;
    leftBackMotor.outVel    = - xPosPid.out  + yPosPid.out  + zPosPid.out ;





    velocityPidUpdata(&leftFrontPid ,leftFrontMotor.outVel - leftFrontMotor.speed);
    velocityPidUpdata(&rightFrontPid,rightFrontMotor.outVel - rightFrontMotor.speed);
    velocityPidUpdata(&rightBackPid ,rightBackMotor.outVel - rightBackMotor.speed );
    velocityPidUpdata(&leftBackPid  ,leftBackMotor.outVel - leftBackMotor.speed);

    if(leftFrontPid.out > 0){LEFTFRONT_MOTOR_CW;}
    else {LEFTFRONT_MOTOR_CCW;}

    if(rightFrontPid.out > 0){RIGHTFRONT_MOTOR_CW;}
    else {RIGHTFRONT_MOTOR_CCW;}

    if(rightBackPid.out > 0){RIGHTBACK_MOTOR_CW;}
    else {RIGHTBACK_MOTOR_CCW;}

    if(leftBackPid.out > 0){LEFTBACK_MOTOR_CW;}
    else {LEFTBACK_MOTOR_CCW;}



    leftFrontMotor.outPwm =  fabs(leftFrontPid.out)  ;
    rightFrontMotor.outPwm = fabs(rightFrontPid.out) ;
    rightBackMotor.outPwm =  fabs(rightBackPid.out)  ;
    leftBackMotor.outPwm  =  fabs(leftBackPid.out)   ;

}


void transmitMotorCtor(uint16_t pwm,uint8_t dir)
{
    if(pwm>1000)
        return;

    if(dir == 0)
    {
        TIM12->CCR1 = 0;
        TRANSMIT_MOTOR_STOP;
    }
    else if(dir == 1)
    {
        TIM12->CCR1 = pwm;
        TRANSMIT_MOTOR_CCW;
    }
    else
    {
        TIM12->CCR1 = pwm;
        TRANSMIT_MOTOR_CW;
    }


}

void motorPidInterClear(void)
{
    pidReset(&leftFrontPid);
    pidReset(&leftBackPid);
    pidReset(&rightFrontPid);
    pidReset(&rightBackPid);

    pidReset(&xPosPid);
    pidReset(&yPosPid);
    pidReset(&zPosPid);

}



